FUNCTION_BLOCK LCB_ActuatorSpeed_V2
TITLE = 'LCB_ActuatorSpeed_V2'

// Functionblock for Lenze Application ActuatorSpeed with 8400 and 9400 Drives. 
// A special Application is needed on the Drive. See documentation for details.
// The implementation is independant of the used bussystem (Profinet / Profibus).
//
// Used S7 Systemfunctions 
// SFC14 (DPRD_DAT)
// SFC15 (DPWR_DAT)
//
// Versionshistory:
// 1.0 Ersterstellung E.Dittmann
// 1.1 Schnittstellenanpassung E.Dittmann
// 2.0 Release

AUTHOR : 'Lenze'
Family : 'LCB'
Version : '2.0'
Name: 'Speed_V2'
KNOW_HOW_PROTECT


CONST
    Range := 4; // Count of wors from / to the drive 
END_CONST

VAR_INPUT
    iFirstPeripherieADR:    INT;                                    // Peripheral adress of the drive (Hardwareconfig S7)
    xEnableInternalControl  : BOOL := FALSE;                        // Enable internal drivecontrol with variabletable
    iDriveType              : INT := 0;                             // 1=84Motec / 2=8400 SL / HL / TL, 11=9400 HL
    xDriveEnable            : BOOL := FALSE;                        // Enable Drive
    xDriveSetQsp            : BOOL := FALSE;                        // Set Quickstop
    xResetError             : BOOL := FALSE;                        // Reset error
    xEnableSpeedSetpoint    : BOOL := FALSE;                        // Enable Speedsetpoint
    xJog1Set                : BOOL := FALSE;                        // JOG 1 Speed
    xJog2Set                : BOOL := FALSE;                        // JOG 2 Speed
    xInvertDirectionSet     : BOOL := FALSE;                        // Reversion of rotation
    rSpeedSetpoint          : REAL := 0.0;                          // Speedsetpoint scaled in % of C11
    xFreeCtrl_1             : BOOL := False;
    xFreeCtrl_2             : BOOL := False;
    xFreeCtrl_3             : BOOL := False;
    xFreeCtrl_4             : BOOL := False;
    wFreeCtrl_1             : Word := 0;

END_VAR

VAR_OUTPUT
    xInternalControlActive  : BOOL;                                 // Internal control active
    xError                  : BOOL;                                 // Error
    xCommunicationOK        : BOOL;                                 // Communication OK
    xDriveError             : BOOL;                                 // Drive Error
    xDriveWarning           : BOOL;                                 // Drive Warning
    xDriveReady             : BOOL;                                 // Drive Ready
    xDriveEnabled           : BOOL;                                 // Drive Enabled 
    xDriveQspActive         : BOOL;                                 // Quickstop active
    xSpeedEqZero            : BOOL;                                 // Speed equal zero
    xDirectionCCW           : BOOL;                                 // Invers rotation
    rSpeedActual            : REAL;                                 // Actual speed scaled in % of C11
    iReadError              : INT;                                  // Errorcode aus SFC14 
    iWriteError             : INT;                                  // Errorcode aus SFC15 
    xFreeState_1             : BOOL := False;
    xFreeState_2             : BOOL := False;
    xFreeState_3             : BOOL := False;
    xFreeState_4             : BOOL := False;
    wFreeState_1             : Word := 0;


END_VAR

VAR_IN_OUT

END_VAR


VAR
    wFirstPeripherieADR:    WORD;                                   
    xDriveEnable_IC            : BOOL := FALSE;
    xDriveSetQsp_IC            : BOOL := FALSE;
    xResetError_IC             : BOOL := FALSE;
    xEnableSpeedSetpoint_IC    : BOOL := FALSE;
    xJog1Set_IC                : BOOL := FALSE;
    xJog2Set_IC                : BOOL := FALSE;
    xInvertDirectionSet_IC     : BOOL := FALSE;
    rSpeedSetpoint_IC          : REAL := 0.0;
    xFreeCtrl_1_IC             : BOOL := False;
    xFreeCtrl_2_IC             : BOOL := False;
    xFreeCtrl_3_IC             : BOOL := False;
    xFreeCtrl_4_IC             : BOOL := False;
    wFreeCtrl_1_IC             : Word := 0;
    iSetpoint                  : INT  := 0;
    diSetpoint                 : DINT := 0;    
    scInternalCtrl:      STRUCT                                  
                                b8:     BOOL:= FALSE;
                                b9:     BOOL:= FALSE;
                                b10:    BOOL:= FALSE;
                                b11:    BOOL:= FALSE;
                                b12:    BOOL:= FALSE;
                                b13:    BOOL:= FALSE;
                                b14:    BOOL:= FALSE;
                                b15:    BOOL:= FALSE;
                                b0:     BOOL:= FALSE;
                                b1:     BOOL:= FALSE;
                                b2:     BOOL:= FALSE;
                                b3:     BOOL:= FALSE;
                                b4:     BOOL:= FALSE;
                                b5:     BOOL:= FALSE;
                                b6:     BOOL:= FALSE;
                                b7:     BOOL:= FALSE;
                                awInt:  ARRAY[2..Range] OF WORD;
                            END_STRUCT;
    wOUT                        :WORD;
    xwOut AT wOut               :ARRAY[0..15] OF BOOL;
                            
    scInternalState:        STRUCT                                  
                                b8:     BOOL;
                                b9:     BOOL;
                                b10:    BOOL;
                                b11:    BOOL;
                                b12:    BOOL;
                                b13:    BOOL;
                                b14:    BOOL;
                                b15:    BOOL;
                                b0:     BOOL;
                                b1:     BOOL;
                                b2:     BOOL;
                                b3:     BOOL;
                                b4:     BOOL;
                                b5:     BOOL;
                                b6:     BOOL;
                                b7:     BOOL;
                                awInt:  ARRAY[2..Range] OF WORD;
                            END_STRUCT;

END_VAR

BEGIN

wFirstPeripherieADR:= INT_TO_WORD(iFirstPeripherieADR);             
iReadError:=0;
iWriteError:=0;

IF xEnableInternalControl THEN                                      // Interne Steuerung des Reglers
    xInternalControlActive:=True;
ELSE                                                               // Steuerung des Reglers über Eingangsvariablen des Bausteins
    xInternalControlActive:=False;
END_IF;



// keine Interne Steuerung aktiv ?
IF NOT xInternalControlActive THEN
    // Eingangssignale auf interne Variablen kopieren
    xDriveEnable_IC             := xDriveEnable ;
    xDriveSetQsp_IC             := xDriveSetQsp;
    xResetError_IC              := xResetError;
    xEnableSpeedSetpoint_IC     := xEnableSpeedSetpoint;
    xJog1Set_IC                 := xJog1Set;
    xJog2Set_IC                 := xJog2Set;
    xFreeCtrl_1_IC              := xFreeCtrl_1;
    xFreeCtrl_2_IC              := xFreeCtrl_2;
    xFreeCtrl_3_IC              := xFreeCtrl_3;
    xFreeCtrl_4_IC              := xFreeCtrl_4;
    wFreeCtrl_1_IC              := wFreeCtrl_1;

    xInvertDirectionSet_IC      := xInvertDirectionSet;
    rSpeedSetpoint_IC           := rSpeedSetpoint;
END_IF;

// Eingangsdaten vom Regler lesen
    iReadError:= DPRD_DAT(
                        LADDR := wFirstPeripherieADR, 
                        RECORD := scInternalState ); 


    CASE iDriveType OF  
        // L-Force 8400 Stateline / Highline / Topline             
        // Statusdaten zuweisen
    2:  xDriveError:=scInternalState.b13 OR (scInternalState.b11 AND NOT scInternalState.b10 AND NOT scInternalState.b9 AND NOT scInternalState.b8);
        xDriveWarning:=scInternalState.b12;
        xDriveReady:=scInternalState.b15;
        xDriveEnabled:=NOT scInternalState.b7;
        xDriveQspActive:=scInternalState.b0;
        xSpeedEqZero:=scInternalState.b6;
        xDirectionCCW:=scInternalState.b14;
        xFreeState_1:=WORD_TO_INT((SHR(IN:=scInternalState.awInt[2],N:=0) AND 16#0001)) > 0;
        xFreeState_2:=WORD_TO_INT((SHR(IN:=scInternalState.awInt[2],N:=1) AND 16#0001)) > 0;
        xFreeState_3:=WORD_TO_INT((SHR(IN:=scInternalState.awInt[2],N:=2) AND 16#0001)) > 0;
        xFreeState_4:=WORD_TO_INT((SHR(IN:=scInternalState.awInt[2],N:=3) AND 16#0001)) > 0;
        wFreeState_1:=scInternalState.awInt[4];
        rSpeedActual:=LIMIT( MN:= -199.99, IN:=(INT_TO_REAL(WORD_TO_INT(scInternalState.awInt[3])) / 163.848), MX:=199.99);
        
        // Steuerdaten zuweisen
        scInternalCtrl.b0:=True;
        //scInternalCtrl.b1:= //Impulssperre setzen;
        scInternalCtrl.b2:=xDriveSetQsp_IC;
        scInternalCtrl.b3:=xDriveEnable_IC;
        //scInternalCtrl.b4:= //Reserved;
        //scInternalCtrl.b5:= //Reserved;
        //scInternalCtrl.b6:= //Reserved;
        scInternalCtrl.b7:=xResetError_IC;
        //scInternalCtrl.b8:= //SetHalt;
        //scInternalCtrl.b9:= //Reserved;
        //scInternalCtrl.b10:=//Reserved;
        //scInternalCtrl.b11:=//Set DC BREAK;
        scInternalCtrl.b12:=xJog1Set_IC;
        scInternalCtrl.b13:=xJog2Set_IC;
        //scInternalCtrl.b14:=//Set FAIL;
        scInternalCtrl.b15:=xInvertDirectionSet_IC;
        
        // zuweisen der freien Controlbits
        wOut:= 0;  // Initialisierung
        xwOut[8]:= xFreeCtrl_1_IC;
        xwOut[9]:= xFreeCtrl_2_IC;
        xwOut[10]:= xFreeCtrl_3_IC;
        xwOut[11]:= xFreeCtrl_4_IC; 
        scInternalCtrl.awInt[2]:= wOut;
                  
        (*Scale setpoint to DINT*)
        rSpeedSetpoint_IC := LIMIT(MN:= -199.99, IN:= rSpeedSetpoint_IC, MX:= 199.99);
        iSetpoint := REAL_TO_INT(LIMIT(MN:= -32768, IN:= (rSpeedSetpoint_IC*163.848), MX:= 32767));

        // behandlung von xEnableSpeedSetpoint
        // wenn nicht gesetzt, dann werden keine Sollwerte an den Drive übertragen
        IF NOT xEnableSpeedSetpoint_IC THEN
            iSetpoint:= 0;
            scInternalCtrl.b12:= FALSE;
            scInternalCtrl.b13:= FALSE;
        END_IF; 
    
        (*Write setpoint to drive*)
        scInternalCtrl.awInt[3]:=INT_TO_WORD(iSetpoint);
        scInternalCtrl.awInt[4]:=wFreeCtrl_1_IC;
        
        // L-Force 8400 Motec              
        // Statusdaten zuweisen
    1:  xDriveError:=scInternalState.b13 OR (scInternalState.b11 AND NOT scInternalState.b10 AND NOT scInternalState.b9 AND NOT scInternalState.b8);
        xDriveWarning:=scInternalState.b12;
        xDriveReady:=scInternalState.b15;
        xDriveEnabled:=NOT scInternalState.b7;
        xDriveQspActive:=scInternalState.b0;
        xSpeedEqZero:=scInternalState.b6;
        xDirectionCCW:=scInternalState.b14;
        xFreeState_1:=WORD_TO_INT((SHR(IN:=scInternalState.awInt[2],N:=0) AND 16#0001)) > 0;
        xFreeState_2:=WORD_TO_INT((SHR(IN:=scInternalState.awInt[2],N:=1) AND 16#0001)) > 0;
        xFreeState_3:=WORD_TO_INT((SHR(IN:=scInternalState.awInt[2],N:=2) AND 16#0001)) > 0;
        xFreeState_4:=WORD_TO_INT((SHR(IN:=scInternalState.awInt[2],N:=3) AND 16#0001)) > 0;
        wFreeState_1:=scInternalState.awInt[4];
        rSpeedActual:=LIMIT( MN:= -199.99, IN:=(INT_TO_REAL(WORD_TO_INT(scInternalState.awInt[3])) / 163.84), MX:=199.99) ;
                                                                                                                            
        // Steuerdaten zuweisen
        scInternalCtrl.b0:=True;
        //scInternalCtrl.b1:= //Impulssperre setzen;
        scInternalCtrl.b2:=xDriveSetQsp_IC;
        scInternalCtrl.b3:=xDriveEnable_IC;
        //scInternalCtrl.b4:= //Reserved;
        //scInternalCtrl.b5:= //Reserved;
        //scInternalCtrl.b6:= //Reserved;
        scInternalCtrl.b7:=xResetError_IC;
        //scInternalCtrl.b8:= //SetHalt;
        //scInternalCtrl.b9:= //Reserved;
        //scInternalCtrl.b10:=//Reserved;
        //scInternalCtrl.b11:=//Set DC BREAK;
        scInternalCtrl.b12:=xJog1Set_IC;
        scInternalCtrl.b13:=xJog2Set_IC;
        //scInternalCtrl.b14:=//Set FAIL;
        scInternalCtrl.b15:=xInvertDirectionSet_IC;
        
        // zuweisen der freien Controlbits
        wOut:= 0;  // Initialisierung
        xwOut[8]:= xFreeCtrl_1_IC;
        xwOut[9]:= xFreeCtrl_2_IC;
        xwOut[10]:= xFreeCtrl_3_IC;
        xwOut[11]:= xFreeCtrl_4_IC; 
        scInternalCtrl.awInt[2]:= wOut;
        
        (*Scale setpoint to DINT*)
        rSpeedSetpoint_IC := LIMIT(MN:= -199.99, IN:= rSpeedSetpoint_IC, MX:= 199.99);
        iSetpoint := REAL_TO_INT(LIMIT(MN:= -32768, IN:= (rSpeedSetpoint_IC*163.84), MX:= 32767));

        // behandlung von xEnableSpeedSetpoint
        // wenn nicht gesetzt, dann werden keine Sollwerte an den Drive übertragen
        IF NOT xEnableSpeedSetpoint_IC THEN
            iSetpoint:= 0;
            scInternalCtrl.b12:= FALSE;
            scInternalCtrl.b13:= FALSE;
        END_IF; 
    
        (*Write setpoint to drive*)
        scInternalCtrl.awInt[3]:=INT_TO_WORD(iSetpoint);
        scInternalCtrl.awInt[4]:=wFreeCtrl_1_IC;



        // L-Force 9400     
        // Statusdaten zuweisen
    11: xDriveError:=scInternalState.b0;
        xDriveWarning:=scInternalState.b12;
        xDriveReady:=scInternalState.b2;
        xDriveEnabled:=NOT scInternalState.b7;
        xDriveQspActive:=scInternalState.b3;
        xSpeedEqZero:=scInternalState.b6;
        xFreeState_1:=scInternalState.b4;
        xFreeState_2:=scInternalState.b5;
        xFreeState_3:=scInternalState.b14;
        xFreeState_4:=scInternalState.b15;
        wFreeState_1:=scInternalState.awInt[2];
        IF (rSpeedActual < 0 AND NOT xSpeedEqZero) THEN
            xDirectionCCW    := TRUE;            
        ELSE
            xDirectionCCW    := FALSE;            
        END_IF;
        
        rSpeedActual := DINT_TO_REAL(WORD_TO_DINT(scInternalState.awInt[4]) + DWORD_TO_DINT(SHL(IN:=WORD_TO_DWORD(scInternalState.awInt[3]),N:=16)))/10737418.240; 
        
        // Steuerdaten zuweisen
        scInternalCtrl.b0:=NOT xDriveEnable_IC;
        scInternalCtrl.b1:=xDriveSetQsp_IC;
        scInternalCtrl.b2:=xEnableSpeedSetpoint_IC;
        scInternalCtrl.b3:=xJog1Set_IC;
        scInternalCtrl.b4:=xJog2Set_IC;
        scInternalCtrl.b5:=xInvertDirectionSet_IC;
        scInternalCtrl.b6:= xFreeCtrl_1_IC;
        scInternalCtrl.b7:= xFreeCtrl_2_IC;
        scInternalCtrl.b8:= xFreeCtrl_3_IC;
        scInternalCtrl.b9:= xFreeCtrl_4_IC;
        scInternalCtrl.b10:=xResetError_IC;
        //scInternalCtrl.b11:=ErrorSet;
        //scInternalCtrl.b12:=free;
        //scInternalCtrl.b13:=EnableManual;
        //scInternalCtrl.b14:=ManualPositive;
        //scInternalCtrl.b15:=ManualNegative;

        // scale setpoint to DINT
        rSpeedSetpoint_IC := LIMIT(MN:=-199.99, IN:=rSpeedSetpoint_IC, MX:=199.99);
        diSetpoint := REAL_TO_DINT(rSpeedSetpoint_IC*10737418.240);
        
        // write setpoint to drive
        scInternalCtrl.awInt[4]:=DINT_TO_WORD(diSetpoint);
        scInternalCtrl.awInt[3]:=DWORD_TO_WORD(SHR(IN:=DINT_TO_DWORD(diSetpoint),N:=16));
        
        scInternalCtrl.awInt[2]:=wFreeCtrl_1_IC;



    ELSE:
        ;
    END_CASE;



// Ausgangsdaten zum Regler schreiben                         
    iWriteError:=DPWR_DAT(
                        LADDR := wFirstPeripherieADR, 
                        RECORD := scInternalCtrl ); 

                                                                    
IF (iReadError <> 0) OR (iWriteError <> 0) THEN
    xCommunicationOK:=false;
ELSE
    xCommunicationOK:=True;
END_IF;

    xError:= NOT xCommunicationOK 
             OR NOT ((iDriveType <> 1) OR (iDriveType <> 2) OR (iDriveType <> 11));
             
     
END_FUNCTION_BLOCK